#ifndef _STLWRITER_H_
#define _STLWRITER_H_

#include <iostream>
#include <vector>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <csignal>
#include <sys/stat.h>
#include <iostream>

#include <rwlibs/task.hpp>

#include <rw/geometry/STLFile.hpp>
#include <rw/geometry/Triangle.hpp>
#include <rw/geometry/PlainTriMesh.hpp>
#include <rw/geometry/TriangleUtil.hpp>
#include <rw/geometry/GeometryFactory.hpp>
#include <rw/rw.hpp>
#include <rw/math/Vector3D.hpp>
#include <rw/math/LinearAlgebra.hpp>

#include <rwsim/dynamics/ContactManifold.hpp>
#include <rwsim/dynamics/ContactPoint.hpp>
#include <rwsim/dynamics/ContactCluster.hpp>
#include <rwsim/dynamics/DynamicUtil.hpp>

#include <boost/lexical_cast.hpp>

USE_ROBWORK_NAMESPACE
using namespace std;
using namespace robwork;
using namespace rwsim::dynamics;
using namespace boost::numeric::ublas;

/*!
 * Class to write collision info to an STL file
 */
class STLWriter {
 public:
  STLWriter();
  ~STLWriter();
  void writeCollisionFile(string stlFileName, std::vector<double> &plane, std::vector<double*> &points, double width);
};

#endif
